#include "PCA9685.h"

/****************************************************
 * 2022年4月8日
 * 吴昕灿
 * STC15W4K65S
 * 对PCA9685初始化函数
 * 通讯方式为I2C
 * SDA P21
 * SCL P20
 ***************************************************/

sbit SCL = GPIO_SCL;
sbit SDA = GPIO_SDA;

extern u8 move_config;

servo_360_com servo_l_up_down;
servo_360_com servo_r_up_down;

u8 move_stat_config;

u8 end_f_flag;
u8 end_b_flag;
u8 end_l_flag;
u8 end_r_flag;


void PCA_Init()
{
	move_stat_config = left_up_move;
	end_f_flag = TURE;
	end_r_flag = TURE;
	end_l_flag = TURE;
	end_b_flag = TURE;
}

void PCA9685_write(uchar address,uchar date) reentrant
{
	I2C_Start();
	I2C_SendByte(PCA9685_adrr);   
	I2C_WaitAck();                          
	I2C_SendByte(address);  	
	I2C_WaitAck();
	I2C_SendByte(date);         
	I2C_WaitAck();
	I2C_Stop();
}

uchar PCA9685_read(uchar address)
{
	uchar date;
	I2C_Start();
	I2C_SendByte(PCA9685_adrr); 
	I2C_WaitAck();
	I2C_SendByte(address);
	I2C_WaitAck();

	I2C_Start();
	I2C_SendByte(PCA9685_adrr|0x01);        
	I2C_WaitAck();
	date=I2C_RecByte();
	I2C_Stop();
	return date;
}





void begin(void)
{
	PCA9685_write(PCA9685_MODE1,0X00);
}




void setPWMFreq(float freq)
{
	uint prescale, oldmode,newmode;
	float prescaleval;
	
	freq *=	0.92;  //校正频率
	prescaleval = 25000000;
	prescaleval /=4096;
	prescaleval	/= freq;
	prescaleval -=1;
	prescale = floor(prescaleval+0.5);

	oldmode = PCA9685_read(PCA9685_MODE1);
	newmode = (oldmode&0xEf)|0x10;
	PCA9685_write(PCA9685_MODE1,newmode);
	PCA9685_write(PCA9685_PRESCALE,prescale);
	PCA9685_write(PCA9685_MODE1,oldmode);
	delayms(2);
	PCA9685_write(PCA9685_MODE1,oldmode|0xa1);
}



/*---------------------------------------------------------------
                                PCA9685修改角度函数
num:舵机PWM输出引脚0~15，on:PWM上升计数值0~4096,off:PWM下降计数值0~4096
一个PWM周期分成4096份，由0开始+1计数，计到on时跳变为高电平，继续计数到off时
跳变为低电平，直到计满4096重新开始。所以当on不等于0时可作延时,当on等于0时，
off/4096的值就是PWM的占空比。
----------------------------------------------------------------*/


void PCA9685_Init(void)
{
	
	GPIO_InitTypeDef GPIO_PCA9685;
	GPIO_PCA9685.Mode = GPIO_OUT_PP;
	GPIO_PCA9685.Pin = GPIO_SCL|GPIO_SDA;
	SCL = 1;
	SDA = 1;
	GPIO_Inilize(GPIO_P2,&GPIO_PCA9685);
	setPWMFreq(50);
	begin();
}


void servo_control(uchar num, float angle) 
{
	//uint off;

	//	off = 410.6+9.502244613*angle;				//LDX-218舵机控制参数 
	uint off = 102+2.2*angle;
	PCA9685_write(LED0_ON_L+4*num,0);
	PCA9685_write(LED0_ON_H+4*num,0);
	PCA9685_write(LED0_OFF_L+4*num,off);
	PCA9685_write(LED0_OFF_H+4*num,off>>8);

	
	
}
//舵机参数
StreetMotor STMotor[5] = 
{
    {15,DEFUAL_ANGLE,DEFUAL_ANGLE,8,NO_TURE},
    {14,DEFUAL_ANGLE,DEFUAL_ANGLE,8,TURE},
    {13,DEFUAL_ANGLE,DEFUAL_ANGLE,DEFUAL_INC,NO_TURE},
    {11,DEFUAL_ANGLE,DEFUAL_ANGLE,DEFUAL_INC,NO_TURE},
    {10,DEFUAL_ANGLE,DEFUAL_ANGLE,DEFUAL_INC,NO_TURE}
};


/*
	舵机扫描
*/
void StreetMotor_Service(void)
{
	uchar temp;
	for(temp=0;temp<5;temp++)
	{
		speed_servo_control(temp,&STMotor);
		
	}
}

/*  
    舵机角度速度控制

    @param num         舵机通道
    @param 	STMotor		舵机参数
 
 */
uchar speed_servo_control(u8 num,StreetMotor*STMotor)
{
	

		STMotor[num].flag = NO_TURE;
		if( STMotor[num].target_angle >= STMotor[num].acc_angle)   //判断目标角度是否小于目标值
		{
			if(STMotor[num].acc_angle + STMotor[num].inc >= STMotor[num].target_angle)  //判断当前角度是否超过目标值（是）
			{	
				STMotor[num].acc_angle = STMotor[num].target_angle;
				servo_control(STMotor[num].channle,STMotor[num].acc_angle);
				STMotor[num].flag = TURE;
				return TURE;
			}
			STMotor[num].acc_angle = STMotor[num].acc_angle + STMotor[num].inc;
			servo_control(STMotor[num].channle,STMotor[num].acc_angle); //当前角度向目标值偏移(+)
			STMotor[num].flag = NO_TURE;
			return NO_TURE;

		}
		else
		{
			if(STMotor[num].acc_angle + STMotor[num].inc <= STMotor[num].target_angle)   //判断当前角度是否超过目标值（是）
			{	
				STMotor[num].acc_angle = STMotor[num].target_angle;
				servo_control(STMotor[num].channle,STMotor[num].acc_angle);
				STMotor[num].flag = TURE;
				return TURE;
			}
			STMotor[num].acc_angle = STMotor[num].acc_angle - STMotor[num].inc;
			servo_control(STMotor[num].channle,STMotor[num].acc_angle);//当前角度向目标值偏移(-)
			STMotor[num].flag = NO_TURE;
			return NO_TURE;
		}
		
	
	return NO_TURE;
}

//舵机360控制
uchar servo_360_control(servo_360_com*servo)
{
	static u16 i = 0;
	i++;
	servo_control(servo->num,servo->speed);
	if((i < servo->time))
	{
		
		
		servo->flag = NO_TURE;
		return servo->flag;
	}
	else
	{
		servo->flag = TURE;
		i =0;
		servo_control(servo->num,90);
		return servo->flag;
		
	}
	
}


//蜘蛛左转函数
void sp_l_m(void)
{

 	if (end_b_flag == NO_TURE)
	{
	StreetMotor_Service();

	if(move_stat_config == left_up_move)
	{
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(1);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_b_move;
        }
	}
	else if(move_stat_config == left_b_move)
	{
		
		STMotor[0].target_angle = 170;
		STMotor[1].target_angle = 10;
		STMotor[2].target_angle = 70;
		STMotor[3].target_angle = 100;

	
		TX1_write2buff(2);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
		
			move_stat_config = left_down_move;
		}
	}
	else if(move_stat_config == left_down_move)
	{
	
		
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=100;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(3);
		
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = right_up_move;
        }
	}
	else if(move_stat_config == right_up_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=100;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(4);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_f_move;
        }
	}
	else if(move_stat_config == left_f_move)
	{
	
			STMotor[1].target_angle = 90;
			STMotor[2].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_f_move;			
		}
		
	}
	else if(move_stat_config == right_f_move)
	{
	
			STMotor[3].target_angle = 90;
			STMotor[0].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_down_move;			
		}
		
	}
	else if(move_stat_config == right_down_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(6);
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			
			end_b_flag = TURE;
			move_stat_config = left_up_move;
			
        }
	}
	TX1_write2buff(end_b_flag);
	}
	
}


//蜘蛛前进
void sp_f_m(void)
{


 if (end_b_flag == NO_TURE)
	{
	StreetMotor_Service();

	if(move_stat_config == left_up_move)
	{
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(1);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_b_move;
        }
	}
	else if(move_stat_config == left_b_move)
	{
		
		STMotor[0].target_angle = 60;
		STMotor[1].target_angle = 60;
		STMotor[2].target_angle = 70;
		STMotor[3].target_angle = 80;

	
		TX1_write2buff(2);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
		
			move_stat_config = left_down_move;
		}
	}
	else if(move_stat_config == left_down_move)
	{
	
		
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(3);
		
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = right_up_move;
        }
	}
	else if(move_stat_config == right_up_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(4);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_f_move;
        }
	}
	else if(move_stat_config == left_f_move)
	{
	
			STMotor[1].target_angle = 90;
			STMotor[2].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_f_move;			
		}
		
	}
	else if(move_stat_config == right_f_move)
	{
	
			STMotor[3].target_angle = 90;
			STMotor[0].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_down_move;			
		}
		
	}
	else if(move_stat_config == right_down_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(6);
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			
			end_b_flag = TURE;
			move_stat_config = left_up_move;
			
        }
	}
	TX1_write2buff(end_b_flag);
	}
}


void sp_b_m()
{
	if (end_b_flag == NO_TURE)
	{
	StreetMotor_Service();

	if(move_stat_config == left_up_move)
	{
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(1);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_b_move;
        }
	}
	else if(move_stat_config == left_b_move)
	{
		
		STMotor[0].target_angle = 130;
		STMotor[1].target_angle = 60;
		STMotor[2].target_angle = 110;
		STMotor[3].target_angle = 100;

	
		TX1_write2buff(2);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
		
			move_stat_config = left_down_move;
		}
	}
	else if(move_stat_config == left_down_move)
	{
	
		
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(3);
		
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = right_up_move;
        }
	}
	else if(move_stat_config == right_up_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(4);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_f_move;
        }
	}
	else if(move_stat_config == left_f_move)
	{
	
			STMotor[1].target_angle = 90;
			STMotor[2].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_f_move;			
		}
		
	}
	else if(move_stat_config == right_f_move)
	{
	
			STMotor[3].target_angle = 90;
			STMotor[0].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_down_move;			
		}
		
	}
	else if(move_stat_config == right_down_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(6);
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			
			end_b_flag = TURE;
			move_stat_config = left_up_move;
			
        }
	}
	TX1_write2buff(end_b_flag);
	}
		


}

void sp_r_m()
{
 	if (end_b_flag == NO_TURE)
	{
	StreetMotor_Service();

	if(move_stat_config == left_up_move)
	{
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(1);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_b_move;
        }
	}
	else if(move_stat_config == left_b_move)
	{
		
		STMotor[0].target_angle = 70;
		STMotor[1].target_angle = 170;
		STMotor[2].target_angle = 110;
		STMotor[3].target_angle = 70;

	
		TX1_write2buff(2);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
		
			move_stat_config = left_down_move;
		}
	}
	else if(move_stat_config == left_down_move)
	{
	
		
		servo_l_up_down.num= 7;//前360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=90;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(3);
		
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = right_up_move;
        }
	}
	else if(move_stat_config == right_up_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=1;
		servo_l_up_down.time=140;
		servo_l_up_down.flag = NO_TURE;
		servo_360_control(&servo_l_up_down);
		TX1_write2buff(4);
		if(servo_l_up_down.flag != NO_TURE)
        {
			move_stat_config = left_f_move;
        }
	}
	else if(move_stat_config == left_f_move)
	{
	
			STMotor[1].target_angle = 90;
			STMotor[2].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_f_move;			
		}
		
	}
	else if(move_stat_config == right_f_move)
	{
	
			STMotor[3].target_angle = 90;
			STMotor[0].target_angle = 90;
			TX1_write2buff(5);
		if((STMotor[0].flag !=NO_TURE)&&(STMotor[2].flag!= NO_TURE)&&(STMotor[1].flag !=NO_TURE)&&(STMotor[3].flag !=NO_TURE))
		{
			move_stat_config = right_down_move;			
		}
		
	}
	else if(move_stat_config == right_down_move)
	{
		servo_l_up_down.num= 8;//后360舵机
		servo_l_up_down.speed=120;
		servo_l_up_down.time=100;
		servo_l_up_down.flag = NO_TURE;
		TX1_write2buff(6);
		servo_360_control(&servo_l_up_down);
		if(servo_l_up_down.flag != NO_TURE)
        {
			
			end_b_flag = TURE;
			move_stat_config = left_up_move;
			
        }
	}
	TX1_write2buff(end_b_flag);
	}
	

}

void PCA9685_interrupt(void) interrupt PCA9685_INTERRPT using 0
{	
	if(move_config ==JI_XIE_BI_INIT) 
	{
		servo_control(7,90);
		servo_control(8,90);
		
	
	}
	else if(move_config ==STAT) 
	{
		end_f_flag = NO_TURE;
		end_b_flag = NO_TURE;
		end_r_flag = NO_TURE;
		end_l_flag = NO_TURE;
	
	}
	else if(move_config == RIGHT_CONFIG)
	{
		//STMotor[3].target_angle = 70;
		sp_r_m();
		//servo_control(15,1);
	}
	else if(move_config == BACK_CONFIG)
	{
		sp_b_m();
		//servo_control(15,90);
	}
	else if(move_config == FOW_CONFIG)
	{
		sp_f_m();
		//servo_control(15,90);
	}
	else if(move_config == LEFT_CONFIG)
	{
        sp_l_m();
		//servo_control(15,179);
	}

	TH0 = SPEED_4>>8;
	TL0 = SPEED_4;

	TF0 = 0;
}




